/* 
 * File:   Simulator.h
 * Author: hadrien
 *
 * Created on 9 mars 2012, 02:18
 */

#ifndef SEARCH_H
#define	SEARCH_H
#include<Eigen/StdVector>
#include"Robot.h"
#include"float.h"
#include"mrpt/slam.h"
#include"actionGenerator.h"
#include "CRangeBearingKFSLAM2DWithCopy.h"
struct my_cmp
{
    bool operator() (const Action& a, const Action& b)
    {
        return a < b;  
    }
};

class ActionSequenceSearch {
public:
    ActionSequenceSearch(const OdometrySensorModel &odo, const RangeSensorModel &sensor);
    Action nextAction(const Robot &robot, const CRangeBearingKFSLAM2DWithCopy &mapping, const mrpt::slam::CLandmarksMap &landmarkMap, Action &best_a, double &value);
};

template<typename ACTIONS_GENERATOR, typename EVALUATION, int DEPTH, int WIDTH>
class TreeSearch {
public:
    typedef typename std::map<Action, Robot, my_cmp, Eigen::aligned_allocator<std::pair<Action, Robot> > > mapActionRobot;
    typedef typename std::vector<Robot, Eigen::aligned_allocator<Robot> > vectorRobot;
    typedef typename std::map<Action, vectorRobot, my_cmp, Eigen::aligned_allocator<std::pair<Action, vectorRobot> > > mapActionVRobot;
    
    TreeSearch(const OdometrySensorModel &odo, const RangeSensorModel &sensor) :
    m_odo(odo),
    m_sensor(sensor) {
    };
    
    mapActionRobot* expandNode(const Robot& robot, const int &number) {
        mapActionRobot* map = new mapActionRobot;
        std::vector<Action> * actions = ACTIONS_GENERATOR::generate(number);
        std::vector<Action>::const_iterator it;
        for (it = actions->begin(); it < actions->end(); it++) {
            //printf("One Robot\n");
            Robot r(robot);
            //printf("Plop\n");
            r.setOdometryErrors(m_odo);
            r.move(*it, SIZE_STEP_S);
            Action a(*it);
            (*map)[a] = r;
        }
        //Free memory
        delete actions;
        return map;
    };
    
    double deeper(const Robot &robot, CRangeBearingKFSLAM2DWithCopy &mapping, const mrpt::slam::CLandmarksMap &landmarkMap, int deep, double currentInformation, Action& bestAction) {
        Robot r(robot);
        if (deep==0) {
            return currentInformation - EVALUATION::evaluate(r, mapping, landmarkMap, m_sensor);
        }
        mapActionRobot * new_nodes = expandNode(robot, WIDTH);
        deep--;
        double bestGain = 0.0;
        double newInformation = EVALUATION::evaluate(r, mapping, landmarkMap, m_sensor);
        for(mapActionRobot::const_iterator it_nodes = new_nodes->begin(); it_nodes != new_nodes->end(); it_nodes++) {
            CRangeBearingKFSLAM2DWithCopy new_mapping;
            new_mapping.clone(mapping);
            double informationGain = deeper(it_nodes->second, new_mapping, landmarkMap, deep, currentInformation, bestAction);     
            //std::cout << "informationGain: " << informationGain << std::endl;
            //std::cout << "action: " << it_nodes->first.velocity << ", " << it_nodes->first.turn_rate << std::endl;
            if(informationGain>bestGain){
                bestGain = informationGain;
                bestAction = it_nodes->first;
            }
        }
        return bestGain;
    };

    void nextAction(const Robot &robot, const CRangeBearingKFSLAM2DWithCopy &mapping, const mrpt::slam::CLandmarksMap &landmarkMap, Action &best_a, double currentInformation, double &value) {
        CRangeBearingKFSLAM2DWithCopy new_mapping;
        new_mapping.clone(mapping);
        value = deeper(robot, new_mapping, landmarkMap, DEPTH, currentInformation, best_a);
        
    };
    
    /*void nextAction(const Robot &robot, const CRangeBearingKFSLAM2DWithCopy &mapping, const mrpt::slam::CLandmarksMap &landmarkMap, Action &best_a, double &value) {
        //Generate Moves
        mapActionVRobot leafs;
        
        //printf("Before expand node\n");
        mapActionRobot* first_detph = expandNode(robot, m_odo, 1, WIDTH);
        mapActionRobot::const_iterator it;
        //Expand the fisrt depth
        for (it = first_detph->begin(); it != first_detph->end(); it++) {
            vectorRobot vector;
            vector.push_back(it->second);
            leafs[it->first] = vector;
        }
        //printf("First Depth!\n");
        delete first_detph;
        //Build the tree
        for (int i = 1; i < DEPTH; i++) {
            mapActionVRobot::iterator it_leafs;
            //std::cout << "Depth " << i << std::endl;
            for (it_leafs = leafs.begin(); it_leafs != leafs.end(); it_leafs++) {      
                //std::cout << "One leaf" << std::endl;
                vectorRobot::const_iterator it_robot;
                vectorRobot vector;
                for (it_robot = it_leafs->second.begin(); it_robot != it_leafs->second.end(); it_robot++) {          
                    mapActionRobot* new_depth;
                    new_depth = expandNode(*it_robot, m_odo, 1, WIDTH);
                    //std::cout << "New depth " << std::endl;
                    for(mapActionRobot::const_iterator it_new_depth = new_depth->begin(); it_new_depth != new_depth->end(); it_new_depth++){
                        vector.push_back(it_new_depth->second);
                    }
                    delete new_depth;
                }
                it_leafs->second = vector;
            }
        }
        value = DBL_MAX;
        mapActionVRobot::iterator it_leafs;
        double score; 
        
        //Sense and select the better first move
        for(it_leafs = leafs.begin(); it_leafs != leafs.end(); it_leafs++){
            for(vectorRobot::iterator it_robot = it_leafs->second.begin(); it_robot != it_leafs->second.end(); it_robot++) {
                score = EVALUATION::evaluate(*it_robot, mapping, landmarkMap, m_sensor);
                if(score < value) {
                    best_a = it->first;
                    value = score;
                }
            }
        }
    };*/
private:
    OdometrySensorModel m_odo;
    RangeSensorModel m_sensor;
};

#endif	/* SIMULATOR_H */

